#include "simulator.h"

Simulator &Simulator::getInstance()
{
	static Simulator instance;
	return instance;
}

Simulator::Simulator() :
	spriteManager(SpriteManager::getInstance()),
	collisionStrategy(new RectangularCollisionStrategy()),
	collisionResponse(new AABBCollisionStrategy()),
	constraints(),
	contactListener(NULL),
	g(Vector2f(0,200)),
	timeStep(1.0/30.0),
	player(NULL)
{
	
}

Simulator::~Simulator()
{
	delete collisionStrategy;
	for(std::list<Constraint *>::iterator it = constraints.begin(); it != constraints.end(); ++it) {
		delete *it;
	}
}

void Simulator::restart()
{
	for(std::list<Constraint *>::iterator it = constraints.begin(); it != constraints.end(); ++it) {
		delete *it;
	}
}

void Simulator::step()
{
	for(std::list<Constraint *>::iterator it = constraints.begin(); it != constraints.end(); ++it) {
		(*it)->SolvePositionConstraint();
		(*it)->SolveVelocityConstraint();
	}

	symplecticEuler();
	checkCollisions();
	satisfyConstraints();
	clearForces();
}

void Simulator::setTimeStep(float t)
{
	timeStep = t;
}

void Simulator::symplecticEuler()
{
	std::list<Sprite *> &objects = spriteManager.getObjectList();
	for(std::list<Sprite *>::iterator it = objects.begin(); it != objects.end(); ++it) {
		if((*it)->getType() == TYPE_STATIC) continue;
		Vector2f c = (*it)->getPosition();
		Vector2f v = (*it)->getVelocity();
		const Vector2f &a = (*it)->getAccel();

		if((*it)->isIgnoreGravity()) {
			v += timeStep * a;
		} else {
			v += timeStep * (a + g);
		}
		c += timeStep * v;

		(*it)->setPosition(c);
		(*it)->setVelocity(v);
	}
}

void Simulator::clearForces()
{
	std::list<Sprite *> &objects = spriteManager.getObjectList();
	for(std::list<Sprite *>::iterator it = objects.begin(); it != objects.end(); ++it) {
		(*it)->setAccel(Vector2f());
	}
}

// Go through all objects and check for collisions
void Simulator::checkCollisions()
{
	std::list<Sprite *> &objects = spriteManager.getObjectList();
	for(std::list<Sprite *>::iterator it1 = objects.begin(); it1 != objects.end(); ++it1) {
		Sprite &o1 = **it1;
		if(o1.isCollidable() && o1.isState(STATE_LIVING) && o1.getType() == TYPE_DYNAMIC) {
			checkObjectCollision(**it1);
		}
	}
}

// Check if the object is colliding with another object
void Simulator::checkObjectCollision(Sprite &o1)
{
	std::list<Sprite *> &objects = spriteManager.getObjectList();
	for(std::list<Sprite *>::iterator it2 = objects.begin(); it2 != objects.end(); ++it2) {
		Sprite &o2 = **it2;
		if( o2.isCollidable() && o2.isState(STATE_LIVING) && (&o1 != &o2) && o1.getOwner() != &o2 && o2.getOwner() != &o1) {
			if( o2.getType() == TYPE_STATIC || ((o2.getType() == TYPE_DYNAMIC && o1.canCollideWithDynamic()) && o2.canCollideWithDynamic()) ) {
				executeCollisionStrategy(o1,o2);
			}
		}
	}
}

void Simulator::executeCollisionStrategy(Sprite &d1, Sprite &d2)
{
	if(collisionStrategy->execute(d1,d2)) {
		Contact cdata(&d1,&d2);
		if(contactListener) {
			if(contactListener->onContact(cdata)) {
				collisionResponse->execute(d1,d2);
			}
		}
	}
}

void Simulator::setContactListener(ContactListener *cl)
{
	contactListener = cl;
}

void Simulator::satisfyConstraints()
{
	Gamedata &gd = Gamedata::getInstance();
	float world_w = gd.getXmlInt("worldWidth");
	float world_h = gd.getXmlInt("worldHeight");

	std::list<Sprite *> &objects = spriteManager.getObjectList();
	for(std::list<Sprite *>::iterator it = objects.begin(); it != objects.end(); ++it) {
		if((*it)->getType() == TYPE_STATIC)
			continue;

		Vector2f c = (*it)->getPosition();
		Vector2f v = (*it)->getVelocity();
		float w = (*it)->getFrame()->getWidth();
		float h = (*it)->getFrame()->getHeight();

		if(*it == player->getSprite()) {
			// Player world constraint
			if(c[0] < 0) {
				c[0] = 0;
				v[0] = 0;
			}
			if(c[1] < 0) {
				c[1] = 0;
				v[1] = 0;
			}
			if(c[0]+w > world_w) {
				c[0] = world_w-w;
				v[0] = 0;
			}
			if(c[1]+h > world_h) {
				c[1] = world_h-h;
				v[1] = 0;
			}
		} else {
			// World Constraint
			if(c[0] < 0) {
				c[0] = 0;
				v[0] = -v[0];
			}
			if(c[1] < 0) {
				c[1] = 0;
				v[1] = -v[1];
			}
			if(c[0]+w > world_w) {
				c[0] = world_w-w;
				v[0] = -v[0];
			}
			if(c[1]+h > world_h) {
				c[1] = world_h-h;
				v[1] = -0.9*v[1];
				v[0] = 0.9*v[0];
			}
		}

		(*it)->setPosition(c);
		(*it)->setVelocity(v);
	}
}

void Simulator::addConstraint(Constraint *d)
{
	constraints.push_back(d);
}

void Simulator::removeConstraint(Constraint *d)
{
	constraints.remove(d);
}

void Simulator::removeConstraints(Sprite *s)
{
	std::list<Constraint *>::iterator it = constraints.begin();
	while( it != constraints.end()) {
		if((*it)->spriteA == s || (*it)->spriteB == s)
			it = constraints.erase(it);
		else
			++it;
	}
}

void Simulator::setPlayer(Player *p)
{
	player = p;
}
